using System.Collections;
using System.Collections.Generic;
using System.Data;
using System.Drawing;
using System.Linq;
using UnityEngine;

public static class Pathfinding {

	public delegate bool CanPassCallBack(Vector2Int position);


	class AStarTempData {
		public Vector2Int point;
		public Vector2Int previous;
		public float distFromStart;
		public float distEstToEnd;
		public float distEstTotal;

		public AStarTempData(
			Vector2Int point,
			Vector2Int previous,
			float distFromStart,
			float distEstToEnd,
			float distEstTotal
		) {
			this.point=point;
			this.previous=previous;
			this.distFromStart=distFromStart;
			this.distEstToEnd=distEstToEnd;
			this.distEstTotal=distEstTotal;
		}

	}
	public class AStarTempDataComparer:IComparer<AStarTempData> {
		int IComparer<AStarTempData>.Compare(AStarTempData x,AStarTempData y) {
			if(x.distEstTotal>y.distEstTotal) return 1;
			if(x.distEstTotal<y.distEstTotal) return -1;
			if(x.point.x>y.point.x) return 1;
			if(x.point.x<y.point.x) return -1;
			if(x.point.y>y.point.y) return 1;
			if(x.point.y<y.point.y) return -1;
			return 0;
		}
	}

	static float EstDistance(Vector2Int from,Vector2Int to) {
		float distX = Mathf.Abs(from.x-to.x);
		float distY = Mathf.Abs(from.y-to.y);
		return Mathf.Min(distX,distY)*Mathf.Sqrt(2f)+Mathf.Abs(distX-distY);
	}

	static List<Vector2Int> _discard = new List<Vector2Int>();

	public static bool PathFind(Vector2Int start,Vector2Int end,ref List<Vector2Int> path,CanPassCallBack canPass,bool getPath = true) {
		SortedSet<AStarTempData> openList = new SortedSet<AStarTempData>(new AStarTempDataComparer());
		Dictionary<Vector2Int,AStarTempData> openListDict = new Dictionary<Vector2Int,AStarTempData>();
		Dictionary<Vector2Int,AStarTempData> closedList = new Dictionary<Vector2Int,AStarTempData>();
		if(!getPath) {
			if(path!=null) path.Clear();
			else path=new List<Vector2Int>();
		}

		if(!canPass(start)) return false;
		if(!canPass(end)) return false;

		var startPoint = new AStarTempData(start,start,0,EstDistance(start,end),EstDistance(start,end));
		openList.Add(startPoint);
		openListDict.Add(start,startPoint);

		for(int _ = 0;_<9999;_++) {
			if(_==9998) {
				Debug.LogError("9998");
				return false;
			}
			if(openList.Count==0) return false;

			AStarTempData current = openList.Min;
			if(!openList.Contains(current)) {
				Debug.Log("");
				var i = openList.First(x => x.point==current.point);
				Debug.Log(i);
			}
			openList.RemoveWhere(x => x.point==current.point);
			openListDict.Remove(current.point);


			closedList.Add(current.point,current);


			if(current.point==end) break;

			for(int x = -1;x<=1;x++) {
				if(!canPass(current.point+new Vector2Int(x,0))) continue;
				for(int y = -1;y<=1;y++) {
					Vector2Int newPoint = current.point+new Vector2Int(x,y);
					if(closedList.ContainsKey(newPoint)) continue;
					if(!canPass(newPoint)) continue;
					if(x!=0&&y!=0) {
						if(!canPass(current.point+new Vector2Int(0,y))) continue;
					}

					float distFromStart = current.distFromStart+Mathf.Sqrt(x*x+y*y);
					float distEstToEnd = EstDistance(newPoint,end);

					if(openListDict.ContainsKey(newPoint)) {
						AStarTempData newTemp = openListDict[newPoint];
						if(distFromStart+distEstToEnd<newTemp.distEstTotal) {
							openList.Remove(newTemp);
							newTemp.previous=current.point;
							newTemp.distFromStart=distFromStart;
							newTemp.distEstToEnd=distEstToEnd;
							newTemp.distEstTotal=distFromStart+distEstToEnd;
							openList.Add(newTemp);
						}

					} else {
						AStarTempData newTemp = new AStarTempData(
							newPoint,
							current.point,
							distFromStart,
							distEstToEnd,
							distFromStart+distEstToEnd
						);
						openList.Add(newTemp);
						openListDict.Add(newPoint,newTemp);

					}


				}
			}
		}

		if(!getPath) return true;
		for(Vector2Int point = end;point!=start;point=closedList[point].previous) {
			path.Add(point);
		}
		path.Reverse();

		return true;
	}




}
